/*************************************************************************
*
*	simulate_position.c
*
*  Copyright 2007,2008 Steve Stancliff
*
*  Part of the TAOSF SSA system.
*
*  This module provides functions for simulating boat status.
*
*
**************************************************************************
*
*  version 1.0.8, 10/17/08, SBS
*   - broken out from simulate_boat
*
*  version 1.0.7, 10/4/08, SBS
*   - documentation
*
*  version 1.0.6, 8/27/08, SBS
*   - use status_SetTimeString()
*   - add random offset to boat start positions
*
*  version 1.0.5, 5/30/08, SBS
*    - move conversions (lat, long, deg, rad, etc) into conversions.h
*    - add conditionals for simulating buoys
*    - cleanup
*
*  version 1.0.4, 5/29/08, SBS
*    - add UpdateBoatSpeed()
*    - add status "direction" = net motion direction
*    - use direction instead of heading in positionfromheadingspeed
*    - move InitBoat here from simulate_engineering
*
*  version 1.0.3, 5/18/08, SBS
*    - add boat rotating into wind during free drift
*
*  version 1.0.2, 5/17/08, SBS
*    - modify drift to include both wind and water 
*    - add turn rate to heading changes
*
*  version 1.0.1, 5/12/08, SBS
*    - add drift
*
*  version 1.0, 1/13/08, SBS
*    - split from simulate_engineering.c
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/
/**
*  @file 
*  @ingroup plat_eng_simulate
*  @brief
*  This module provides functions for simulating boat status.
*  <hr>
**/

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>

#include "waypoints.h"
#include "platform_types.h"
#include "platform_modes.h"
#include "status.h"
#include "conversions.h"

//#include "../initialize.h" 

//#include "science_main.h"

//#include "boatmodes.h"

#include "simulate_position.h"


// contributions of water current and wind to drift
//#define DRIFT_FROM_WATER 0.4
//#define DRIFT_FROM_WIND  0.1
//#define DRIFT_FROM_WATER 0.
//#define DRIFT_FROM_WIND  0.

// rotational rate for boat sitting still (deg/s)
//#define HDGROTINC 1.5

// turn rate for moving boat (deg/s)
//#define HDGTURNRATE 12.0



/********************************************************
*
* PositionFromHeadingSpeed()
*/
/**
* @brief
* Given current location, direction, speed, and 
*    elapsed time, calculate new location.
*
********************************************************/
static POSITION PositionFromHeadingSpeed(
			STATUS s
			,int deltat)
{
	POSITION p;
	double distance,hdgRad,latRad,lonRad,newLat,newLon;
		
	distance = (s.dSpeed_mps) * (double)deltat;
	
//	printf("distance covered = %lf\n",distance);

//	hdgRad = RADIANS(s.dHdgDeg);		
	hdgRad = RADIANS(s.dDirectionDeg);		

	latRad = RADIANS(s.dLatDeg);
	lonRad = RADIANS(s.dLonDeg);


	if(s.dDirectionDeg == 90){
		newLat = latRad;
	}else if(s.dDirectionDeg == 270){
		newLat = latRad;
	}else{
		newLat = asin(sin(latRad)*cos(distance/EARTHRADIUS) 
					+ cos(latRad)*sin(distance/EARTHRADIUS)*cos(hdgRad));
	}					
	
		
	if(s.dDirectionDeg == 0){
		newLon = lonRad;
	}else if(s.dDirectionDeg == 180){
		newLon = lonRad;
	}else{
		newLon = lonRad 
				+ atan2(sin(hdgRad)*sin(distance/EARTHRADIUS)*cos(latRad)
							, cos(distance/EARTHRADIUS)-sin(latRad)*sin(newLat));
	}						


	p.dLonDeg = DEGREES(newLon);				
	p.dLatDeg = DEGREES(newLat);


//	printf("oldlat = %12.10lf\n",s.dLatDeg);
//	printf("newlat = %12.10lf\n",p.dLatDeg);
//	printf("oldlon = %12.10lf\n",s.dLonDeg );
//	printf("newlon = %12.10lf\n",p.dLonDeg);		

	return(p);
}


/********************************************************
*
* UpdateBoatPosition()
*/
/**
* @brief
* Determine new location of boat
*
*
********************************************************/
void UpdateBoatPosition_sim(
			STATUS *s
			, int deltat)
{
	

	if(s->dSpeed_mps > 0){
		POSITION p;
		
		p = PositionFromHeadingSpeed(*s,deltat);

		s->dLonDeg = p.dLonDeg;				
		s->dLatDeg = p.dLatDeg;

	}
	
	return;
}


